﻿
#include "obj-func.h"

using namespace objcalc;

void objcalc::calObjVelAndTrack(ObjectSensorSample currentSample, Optional<ObjectSensorSample> lastSampleO, UInt& idCounter, Bool calcVelocity, Bool calcAccel, OptionalDouble vehiSpeed, OptionalDouble vehiAX, OptionalDouble vehiYawRate, OptionalDouble vehiCurv)
{
	if (!lastSampleO.isValid())
	{
		for (UInt i = 0; i < currentSample->objects.size(); i++)
		{
			ObjectInfo& info = currentSample->objects[i];
			info.id = ++idCounter;
			info.age = 1;
			info.trajectory = Array<Point2D>::scalar(Point2D((Float)info.posx, (Float)info.posy));
		}
		return;
	}

	ObjectSensorSample lastSample = lastSampleO.refValue();

	// Get vehicle params
	double vehicleVX = vehiSpeed.valid ? vehiSpeed.value : 0;
	double vehicleAX = vehiAX.valid ? vehiAX.value : 0;

	double vehicleYR = 0;
	if (vehiYawRate.valid) vehicleYR = vehiYawRate.value;
	else if (vehiCurv.valid && math::abs(vehicleVX) > 1)
	{
		vehicleYR = vehiCurv.value * (vehicleVX / 3.6) * math::RAD2DEG;
	}

	// Constants
	const double TRACK_DABS_THRESHOLD = 3.0;
	const double TRACK_VX_THRESHOLD = 5.0;
	const double TRACK_VY_THRESHOLD = 5.0;

	//Delta time
	double deltaT = currentSample.timestamp().offset - lastSample.timestamp().offset;

	//Theta angle
	double theta = vehicleYR * deltaT;

	//cos(theta angle), sin(theta angle)
	double cosTheta = math::cos(theta);
	double sinTheta = math::sin(theta);

	//Ego vehicle moved x/y
	double rp = lastSample->vehiFrontOverhang.valid && lastSample->vehiWheelBase.valid ? (lastSample->vehiFrontOverhang.value + lastSample->vehiWheelBase.value) : 0;
	double x1 = -rp;
	double x2 = x1 + vehicleVX / 3.6 * deltaT;
	double y2 = vehicleVX / 3.6 * math::tan(theta / 2.0) * deltaT;
	double mx = x2 + rp * cosTheta;
	double my = y2 + rp * sinTheta;

	double preDx, preDy, curDx, curDy, preVx, preVy;
	double preTx, preTy;
	double deltaDx, deltaDy, deltaD2, deltaVx, deltaVy;

	//time coef
	const double vxTimeCoef = 0.02; // 20ms
	const double axTimeCoef = 0.05; // 50ms

	double kvx = vxTimeCoef / (vxTimeCoef + deltaT);
	double kax = axTimeCoef / (axTimeCoef + deltaT);

	UInt lastCount = lastSample->objects.size();
	UInt curCount = currentSample->objects.size();
	FloatMat distMat(lastCount, curCount);
	distMat.set(10000.0f);

	for (UInt i = 0; i < lastCount; i++)
	{
		ObjectInfo& lastInfo = lastSample->objects[i];
		OptionalInt lastPbid = lastInfo.rawID;

		//Object previous vx, vy
		preVx = lastInfo.vxAbs.valid ? lastInfo.vxAbs.value : 0;
		preVy = lastInfo.vyAbs.valid ? lastInfo.vyAbs.value : 0;

		//Object predictive location under previous coordinate
		preDx = lastInfo.posx + preVx / 3.6 * deltaT - mx;
		preDy = lastInfo.posy + preVy / 3.6 * deltaT - my;

		//Object predictive location under current coordinate
		curDx = preDx * cosTheta + preDy * sinTheta;
		curDy = -preDx * sinTheta + preDy * cosTheta;

		for (UInt j = 0; j < curCount; j++)
		{
			ObjectInfo& currentInfo = currentSample->objects[j];

			deltaDx = math::abs(currentInfo.posx - curDx);
			deltaDy = math::abs(currentInfo.posy - curDy);
			deltaD2 = deltaDx * deltaDx + deltaDy * deltaDy;

			deltaVx = 0;
			if (currentInfo.vxAbs.valid && lastInfo.vxAbs.valid) deltaVx = math::abs(currentInfo.vxAbs.value - lastInfo.vxAbs.value);
			else if (currentInfo.vxRel.valid && lastInfo.vxRel.valid) deltaVx = math::abs(currentInfo.vxRel.value - lastInfo.vxRel.value);

			deltaVy = 0;
			//if (currentInfo.vyAbs.valid && lastInfo.vyAbs.valid) deltaVy = math::abs(currentInfo.vyAbs.value - lastInfo.vyAbs.value);
			//else if (currentInfo.vyRel.valid && lastInfo.vyRel.valid) deltaVy = math::abs(currentInfo.vyRel.value - lastInfo.vyRel.value);

			//ID matched or Matching failed
			if (lastPbid.valid)
			{
				if (!currentInfo.rawID.valid || currentInfo.rawID.value != lastPbid.value) continue;
				distMat(i, j) = 0;
			}
			else
			{
				if (currentInfo.id != 0 || deltaD2 >= TRACK_DABS_THRESHOLD * TRACK_DABS_THRESHOLD ||
					deltaVx >= TRACK_VX_THRESHOLD || deltaVy >= TRACK_VY_THRESHOLD ||
					currentInfo.classs != lastInfo.classs) continue;
				distMat(i, j) = (Float)deltaD2;
			}
		}
	}

	Array<OptionalInt> lastMap(lastCount);
	for (UInt i = 0; i < lastCount; i++)
	{
		OptionalInt index;
		Float minDist = 9999.0f;
		for (UInt n = 0; n < curCount; n++)
		{
			Float dist = distMat(i, n);
			if (dist < minDist)
			{
				minDist = dist;
				index = n;
			}
		}
		lastMap[i] = index;
	}

	Array<OptionalInt> curMap(curCount);
	for (UInt i = 0; i < curCount; i++)
	{
		OptionalInt index;
		Float minDist = 9999.0f;
		for (UInt n = 0; n < lastCount; n++)
		{
			Float dist = distMat(n, i);
			if (dist < minDist)
			{
				minDist = dist;
				index = n;
			}
		}
		curMap[i] = index;
	}

	for (UInt i = 0; i < curCount; i++)
	{
		if (!curMap[i].valid) continue;
		UInt j = curMap[i].value;
		if (!lastMap[j].valid || lastMap[j].value != (Int)i) continue;

		ObjectInfo& currentInfo = currentSample->objects[i];
		ObjectInfo& lastInfo = lastSample->objects[j];

		//Object previous vx, vy
		preVx = lastInfo.vxAbs.valid ? lastInfo.vxAbs.value : 0;
		preVy = lastInfo.vyAbs.valid ? lastInfo.vyAbs.value : 0;

		//Object predictive location under previous coordinate
		preDx = lastInfo.posx + preVx / 3.6 * deltaT - mx;
		preDy = lastInfo.posy + preVy / 3.6 * deltaT - my;

		//Object predictive location under current coordinate
		curDx = preDx * cosTheta + preDy * sinTheta;
		curDy = -preDx * sinTheta + preDy * cosTheta;

		//Update id and age
		currentInfo.id = lastInfo.id;
		currentInfo.age = lastInfo.age + 1;

		//Calculate vx/vy
		if (calcVelocity)
		{
			if (!currentInfo.vxRel.valid)
			{
				Double newValue = (currentInfo.posx - lastInfo.posx) * 3.6 / deltaT;
				if (lastInfo.vxRel.valid) currentInfo.vxRel = lastInfo.vxRel.value * kvx + newValue * (1.0 - kvx);
				else currentInfo.vxRel = newValue;
				currentInfo.vxAbs = currentInfo.vxRel.value + vehicleVX;
			}
			if (!currentInfo.vyRel.valid)
			{
				Double newValue = (currentInfo.posy - lastInfo.posy) * 3.6 / deltaT;
				if (lastInfo.vyRel.valid) currentInfo.vyRel = lastInfo.vyRel.value * kvx + newValue * (1.0 - kvx);
				else currentInfo.vyRel = newValue;
			}
		}

		//Calculate ax/ay
		if (calcAccel)
		{
			if (!currentInfo.axRel.valid && currentInfo.vxRel.valid && lastInfo.vxRel.valid)
			{
				Double newValue = (currentInfo.vxRel.value - lastInfo.vxRel.value) / 3.6 / deltaT;
				if (lastInfo.axRel.valid) currentInfo.axRel = lastInfo.axRel.value * kax + newValue * (1.0 - kax);
				else currentInfo.axRel = newValue;
				currentInfo.axAbs = currentInfo.axRel.value + vehicleAX;
			}
			if (!currentInfo.ayRel.valid && currentInfo.vyRel.valid && lastInfo.vyRel.valid)
			{
				Double newValue = (currentInfo.vyRel.value - lastInfo.vyRel.value) / 3.6 / deltaT;
				if (lastInfo.ayRel.valid) currentInfo.ayRel = lastInfo.ayRel.value * kax + newValue * (1.0 - kax);
				else currentInfo.ayRel = newValue;
			}
		}

		//Updata track length
		Array<Point2D> output(math::min(100u, lastInfo.trajectory.size() + 1));

		//Updata track
		output[0].x = (Float)currentInfo.posx;
		output[0].y = (Float)currentInfo.posy;

		for (int l = 0; l < ((int)output.size() - 1); l++)
		{
			//Location under previous coordinate
			preTx = lastInfo.trajectory[l].x - mx;
			preTy = lastInfo.trajectory[l].y - my;

			//Location under current coordinate
			output[l + 1].x = (Float)(preTx * cosTheta + preTy * sinTheta);
			output[l + 1].y = (Float)(-preTx * sinTheta + preTy * cosTheta);
		}
		currentInfo.trajectory = output;
	}

	//Updata new object
	for (UInt j = 0; j < currentSample->objects.size(); j++)
	{
		ObjectInfo& info = currentSample->objects[j];
		if (info.id != 0) continue;

		info.id = ++idCounter;
		info.age = 1;
		info.trajectory = Array<Point2D>::scalar(Point2D((Float)info.posx, (Float)info.posy));
	}
}

void objcalc::calAbsVelAndAccel(ObjectSensorSample currentSample, OptionalDouble vehiSpeed, OptionalDouble vehiCurv, OptionalDouble vehiAX, OptionalDouble vehiAY, OptionalDouble vehiOverhang, OptionalDouble vehiWheelbase)
{
	Double rearPos = (vehiOverhang.valid ? vehiOverhang.value : 0) + (vehiWheelbase.valid ? vehiWheelbase.value : 0);

	for (UInt i = 0; i < currentSample->objects.size(); i++)
	{
		ObjectInfo& obj = currentSample->objects[i];
		if (!obj.vxAbs.valid && obj.vxRel.valid && vehiSpeed.valid && vehiCurv.valid)
		{
			obj.vxAbs = obj.vxRel.value + vehiSpeed.value * (1.0 - obj.posy * vehiCurv.value);
		}
		if (!obj.vyAbs.valid && obj.vyRel.valid && vehiSpeed.valid && vehiCurv.valid)
		{
			obj.vyAbs = obj.vyRel.value + vehiSpeed.value * (obj.posx + rearPos) * vehiCurv.value;
		}
		if (!obj.axAbs.valid && obj.axRel.valid && vehiAX.valid)
		{
			obj.axAbs = obj.axRel.value + vehiAX.value;
		}
		if (!obj.ayAbs.valid && obj.ayRel.valid && vehiAY.valid)
		{
			obj.ayAbs = obj.ayRel.value + vehiAY.value;
		}
	}
}

void objcalc::calObjHeadingAndSteering(ObjectSensorSample currentSample, Optional<ObjectSensorSample> lastSampleO, OptionalDouble vehiYawRate)
{
	if (!lastSampleO.isValid()) return;
	ObjectSensorSample lastSample = lastSampleO.value();

	const Double minV = 3.0; // km/h
	const Double timeCoefHeading = 0.1; // 100ms
	const Double timeCoefYawRate = 0.5; // 500ms

	Double deltaT = currentSample.timestamp().offset - lastSample.timestamp().offset;
	Double kHeading = timeCoefHeading / (timeCoefHeading + deltaT);
	Double kYawRate = timeCoefYawRate / (timeCoefYawRate + deltaT);

	for (UInt i = 0; i < currentSample->objects.size(); i++)
	{
		ObjectInfo& currentInfo = currentSample->objects[i];

		// Query heading, yaw rate of previous frame / 获取上帧heading, yawrate等数据
		OptionalDouble lastHeading;
		Point2D lastHeadingV;
		OptionalDouble lastYawRate;
		for (UInt j = 0; j < lastSample->objects.size(); j++)
		{
			ObjectInfo& lastInfo = lastSample->objects[j];
			if (lastInfo.id != currentInfo.id) continue;

			if (lastInfo.heading.valid)
			{
				lastHeading = lastInfo.heading.value;
				lastHeadingV.x = math::cos((Float)lastInfo.heading.value);
				lastHeadingV.y = math::sin((Float)lastInfo.heading.value);
			}
			if (lastInfo.yawRate.valid)
			{
				lastYawRate = lastInfo.yawRate.value;
			}
		}

		// Calculate heading / 计算heading
		if (!currentInfo.heading.valid)
		{
			OptionalDouble currentHeading;
			Point2D currentHeadingV;
			if (currentInfo.vxAbs.valid && currentInfo.vyAbs.valid)
			{
				Double v = math::root(currentInfo.vxAbs.value * currentInfo.vxAbs.value + currentInfo.vyAbs.value * currentInfo.vyAbs.value);
				if (v >= minV)
				{
					currentHeading = math::atan(currentInfo.vyAbs.value, currentInfo.vxAbs.value);
					currentHeadingV.x = (Float)(currentInfo.vxAbs.value / v);
					currentHeadingV.y = (Float)(currentInfo.vyAbs.value / v);
				}
			}

			if (currentHeading.valid)
			{
				if (lastHeading.valid)
				{
					Point2D headingV;
					headingV.x = (Float)(currentHeadingV.x * (1.0 - kHeading) + lastHeadingV.x * kHeading);
					headingV.y = (Float)(currentHeadingV.y * (1.0 - kHeading) + lastHeadingV.y * kHeading);
					if (headingV.x != 0 || headingV.y != 0)
					{
						currentInfo.heading = math::atan(headingV.y, headingV.x);
					}
					else
					{
						currentInfo.heading = lastHeading.value;
					}
				}
				else
				{
					currentInfo.heading = currentHeading.value;
				}
			}
			else
			{
				if (lastHeading.valid)
				{
					currentInfo.heading = lastHeading.value;
				}
				else
				{
					currentInfo.heading.valid = FALSE;
				}
			}
		}

		// Calculate yaw rate / 计算yaw rate
		if (!currentInfo.yawRate.valid)
		{
			OptionalDouble currentYawRate;
			if (currentInfo.heading.valid && lastHeading.valid)
			{
				Double deltaHeading = currentInfo.heading.value - lastHeading.value;
				while (deltaHeading < -180) deltaHeading += 360;
				while (deltaHeading > 180) deltaHeading -= 360;
				currentYawRate = deltaHeading / deltaT + (vehiYawRate.valid ? vehiYawRate.value : 0);
			}

			if (currentYawRate.valid)
			{
				if (lastYawRate.valid)
				{
					currentInfo.yawRate = currentYawRate.value * (1.0 - kYawRate) + lastYawRate.value * kYawRate;
				}
				else
				{
					currentInfo.yawRate = currentYawRate.value;
				}
			}
			else
			{
				if (lastYawRate.valid)
				{
					currentInfo.yawRate = lastYawRate.value;
				}
				else
				{
					currentInfo.yawRate.valid = FALSE;
				}
			}
		}

		// Calculate curvature / 计算curvature
		if (!currentInfo.curvature.valid)
		{
			OptionalDouble speed = currentInfo.speed();
			if (currentInfo.yawRate.valid && speed.valid && speed.value > 5)
			{
				Double curv = currentInfo.yawRate.value * math::DEG2RAD / (speed.value / 3.6);
				if (math::abs(curv) < 1) currentInfo.curvature = curv;
			}
		}
	}
}

Double objcalc::calDistanceToVehicle(Point2D pt, Double vehicleWidth, Double vehicleLength)
{
	if (pt.x <= 0 && pt.x >= -vehicleLength)
	{
		return math::max(0.0, math::abs(pt.y) - vehicleWidth / 2);
	}
	else if (pt.y <= vehicleWidth / 2 && pt.y >= -vehicleWidth / 2)
	{
		if (pt.x >= 0) return pt.x;
		else return math::max(0.0, -vehicleLength - pt.x);
	}
	else if (pt.x >= 0)
	{
		Double py = math::abs(pt.y) - vehicleWidth / 2;
		return math::root(pt.x * pt.x + py * py);
	}
	else // pt.x < -vehicleLength
	{
		Double px = -vehicleLength - pt.x;
		Double py = math::abs(pt.y) - vehicleWidth / 2;
		return math::root(px * px + py * py);
	}
}

void objcalc::calObjClosestPoint(ObjectSensorSample currentSample, OptionalDouble vehiWid, OptionalDouble vehiLen)
{
	Double vehiWidth = vehiWid.valid ? vehiWid.value : 1.9;
	Double vehiLength = vehiLen.valid ? vehiLen.value : 4.6;

	const Double step = 0.05;

	for (UInt i = 0; i < currentSample->objects.size(); i++)
	{
		ObjectInfo& obj = currentSample->objects[i];

		// calculate cpx, cpy / 计算cpx, cpy
		if (obj.cpx == 0 && obj.cpy == 0)
		{
			if (obj.posMode == PositionMode::ClosestPoint)
			{
				obj.cpx = obj.posx;
				obj.cpy = obj.posy;
			}
			else // obj.posMode == PM_BoxCenter
			{
				ObjectClass largeClass = obj.category();
				if (largeClass == ObjectClass::Ped)
				{
					obj.cpx = obj.posx;
					obj.cpy = obj.posy;
				}
				else
				{
					Array<Point2D> corners = ObjectSensorSampleData::calBoxCorners(obj, 0.98);

					Double cpx = 0, cpy = 0;
					Double cpDistSquare = DINF;
					for (UInt a = 0; a < 8; a++)
					{
						Point2D& src = corners[a];
						Point2D& dst = corners[(a + 1) % 8];

						Double len = math::root(math::square(src.x - dst.x) + math::square(src.y - dst.y));
						Double ex = (dst.x - src.x) / len * step;
						Double ey = (dst.y - src.y) / len * step;
						Double nSteps = math::floor(len / step) + 1;

						for (UInt n = 0; n < nSteps; n++)
						{
							Double ptx = src.x + ex * n;
							Double pty = src.y + ey * n;
							Double distSquare = ptx * ptx + pty * pty;
							if (distSquare < cpDistSquare)
							{
								cpx = ptx;
								cpy = pty;
								cpDistSquare = distSquare;
							}
						}
					}

					obj.cpx = cpx;
					obj.cpy = cpy;
				}
			}
		}

		// Calculate cpd / 计算cpd
		obj.cpd = calDistanceToVehicle(Point2D((Float)obj.posx, (Float)obj.posy), vehiWidth, vehiLength);
	}
}

struct CalObjectScore
{
	Int id;
	Double scorePos; // 0~, no scale
	Double scoreNeg; // 0~, 1 as threshold

	CalObjectScore() : id(0), scorePos(0), scoreNeg(0)
	{}
};

CalObjectScore calKOFrontScore(Int id, Double cpx, OptionalDouble overlap)
{
	CalObjectScore score;
	score.id = id;
	score.scorePos = 0;
	score.scoreNeg = 1000000;

	if (cpx <= 0) return score;
	if (!overlap.valid) return score;

	Double ovl = overlap.value;
	if (ovl < -cpx)
	{
		score.scoreNeg = math::max(0.0, -100.0 * (ovl + cpx) / (2500.0 + cpx * cpx));
		return score;
	}

	score.scorePos = math::max(0.0, 1000000 - cpx * cpx + ovl * 25);
	score.scoreNeg = 0;
	return score;
}

OptionalInt calKOIndex(Array<CalObjectScore> scores, OptionalInt lastID)
{
	CalObjectScore lastScore;
	OptionalInt lastIndex;
	if (lastID.valid)
	{
		for (UInt i = 0; i < scores.size(); i++)
		{
			if (scores[i].id == lastID.value)
			{
				lastScore = scores[i];
				lastIndex = i;
				break;
			}
		}
	}

	Double thresh = lastIndex.valid ? lastScore.scorePos : 0;
	OptionalInt newIndex;
	for (UInt i = 0; i < scores.size(); i++)
	{
		if (scores[i].scorePos > thresh)
		{
			newIndex = i;
			thresh = scores[i].scorePos;
		}
	}

	if (newIndex.valid) return newIndex;
	else if (lastIndex.valid)
	{
		if (lastScore.scoreNeg > 1) return OptionalInt();
		else return lastIndex;
	}
	else return OptionalInt();
}

OptionalInt objcalc::findKOFront(ObjectSensorSample sample, OptionalInt lastID)
{
	Array<ObjectInfo> objects = sample->objects;
	Array<CalObjectScore> scores(objects.size());
	for (UInt i = 0; i < objects.size(); i++)
	{
		ObjectInfo& obj = objects[i];
		scores[i] = calKOFrontScore(obj.id, obj.cpx, sample->getOverlap(obj));
	}
	return calKOIndex(scores, lastID);
}

OptionalInt objcalc::findKOSide(ObjectSensorSample sample, Bool isRightSide)
{
	Array<ObjectInfo> objects = sample->objects;
	Array<UInt> prior(objects.size()); // 0:invalid, 1:LC0~3m, 2:LC<0m, 3:LC>4m, 
	for (UInt i = 0; i < objects.size(); i++)
	{
		ObjectInfo& obj = objects[i];
		if (isRightSide && obj.posy >= 0) prior[i] = 0;
		else if (!isRightSide && obj.posy <= 0) prior[i] = 0;
		else if (obj.vxAbs.valid && obj.vxAbs.value < 0) prior[i] = 0;
		else
		{
			OptionalDouble lc = sample->getLC(obj);
			OptionalDouble rc = sample->getRC(obj);
			if (!rc.valid || rc.value > 80) prior[i] = 0;
			else if (!lc.valid) prior[i] = 0;
			else if (lc.value > 0 && lc.value < 4) prior[i] = 1;
			else if (lc.value >= 4) prior[i] = 3;
			else prior[i] = 2;
		}
	}

	OptionalInt index;
	for (UInt p = 1; p <= 3; p++)
	{
		if (index.valid) break;

		Double closestRC = DINF;
		for (UInt i = 0; i < objects.size(); i++)
		{
			ObjectInfo& obj = objects[i];
			if (prior[i] != p) continue;
			Double rcabs = math::abs(sample->getRC(obj).value);
			if (rcabs < closestRC)
			{
				closestRC = rcabs;
				index = i;
			}
		}
	}

	return index;
}